#include "CpFunctions.h"

#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix_double.h>
#include <TFile.h>
#include <TF3.h>
#include <CpPolynom3D.h>
#include <TGeoManager.h>
// ClassImp(CpFunctions);

TF3 *CpFunctions::fgMagForceX = 0;
TF3 *CpFunctions::fgMagForceY = 0;
TF3 *CpFunctions::fgMagForceZ = 0;

TF3 *CpFunctions::fgMagForceXdX = 0;
TF3 *CpFunctions::fgMagForceXdY = 0;
TF3 *CpFunctions::fgMagForceXdZ = 0;
TF3 *CpFunctions::fgMagForceYdX = 0;
TF3 *CpFunctions::fgMagForceYdY = 0;
TF3 *CpFunctions::fgMagForceYdZ = 0;
TF3 *CpFunctions::fgMagForceZdX = 0;
TF3 *CpFunctions::fgMagForceZdY = 0;
TF3 *CpFunctions::fgMagForceZdZ = 0;

TF3 *CpFunctions::fgMagForceXdX2 = 0;
TF3 *CpFunctions::fgMagForceXdY2 = 0;
TF3 *CpFunctions::fgMagForceXdZ2 = 0;
TF3 *CpFunctions::fgMagForceYdX2 = 0;
TF3 *CpFunctions::fgMagForceYdY2 = 0;
TF3 *CpFunctions::fgMagForceYdZ2 = 0;
TF3 *CpFunctions::fgMagForceZdX2 = 0;
TF3 *CpFunctions::fgMagForceZdY2 = 0;
TF3 *CpFunctions::fgMagForceZdZ2 = 0;

CpFunctions::EFunctionType CpFunctions::fgType = CpFunctions::kReal;

Double_t CpFunctions::kMuZero = 1.26663706e-6;
Double_t CpFunctions::kIs = 0.4;
Double_t CpFunctions::kParticleRho = 5*1e3;
Double_t CpFunctions::kParticleR = 50*1e-9;
Double_t CpFunctions::kBloodViscosity = 5.0*1e-3;

Double_t CpFunctions::kBloodVelX = 0.0;
Double_t CpFunctions::kBloodVelY = 0.0;
Double_t CpFunctions::kBloodVelZ = 1e-2;
Double_t CpFunctions::kBulharConstant = 1.0;
Double_t CpFunctions::kMultipier = 1e6;
Bool_t CpFunctions::fgIsInMaget = kFALSE;

//______________________________________________________________________________
CpFunctions::CpFunctions() : TObject() {
//
// Constructor
//

    printf ( "%d\n",GSL_SUCCESS );
}

//______________________________________________________________________________
CpFunctions::~CpFunctions() {
//
// Destructor.
//
}

void CpFunctions::Init() {
    printf ( "Opening function XXXXX ...\n" );
    TFile *f = TFile::Open ( "out3D.root" );
    if ( f ) {
        fgMagForceX = ( TF3* ) f->Get ( "myPol_3D_X" );
        fgMagForceY = ( TF3* ) f->Get ( "myPol_3D_Y" );
        fgMagForceZ = ( TF3* ) f->Get ( "myPol_3D_Z" );
        Double_t *parX = fgMagForceX->GetParameters();
        Double_t *parY = fgMagForceY->GetParameters();
        Double_t *parZ = fgMagForceZ->GetParameters();

        // first derivates
        fgMagForceXdX = new TF3 ( "XdX",&CpPolynom3D::DerivateXDegree10,fgMagForceX->GetXmin(),fgMagForceX->GetXmax(),fgMagForceX->GetYmin(),fgMagForceX->GetYmax(),fgMagForceX->GetZmin(),fgMagForceX->GetZmax(),fgMagForceX->GetNpar() );
        fgMagForceXdX->SetParameters ( parX );
        fgMagForceXdY = new TF3 ( "XdY",&CpPolynom3D::DerivateYDegree10,fgMagForceX->GetXmin(),fgMagForceX->GetXmax(),fgMagForceX->GetYmin(),fgMagForceX->GetYmax(),fgMagForceX->GetZmin(),fgMagForceX->GetZmax(),fgMagForceX->GetNpar() );
        fgMagForceXdY->SetParameters ( parX );
        fgMagForceXdZ = new TF3 ( "XdZ",&CpPolynom3D::DerivateZDegree10,fgMagForceX->GetXmin(),fgMagForceX->GetXmax(),fgMagForceX->GetYmin(),fgMagForceX->GetYmax(),fgMagForceX->GetZmin(),fgMagForceX->GetZmax(),fgMagForceX->GetNpar() );
        fgMagForceXdZ->SetParameters ( parX );

        fgMagForceYdX = new TF3 ( "YdX",&CpPolynom3D::DerivateXDegree10,fgMagForceY->GetXmin(),fgMagForceY->GetXmax(),fgMagForceY->GetYmin(),fgMagForceY->GetYmax(),fgMagForceY->GetZmin(),fgMagForceY->GetZmax(),fgMagForceY->GetNpar() );
        fgMagForceYdX->SetParameters ( parY );
        fgMagForceYdY = new TF3 ( "YdY",&CpPolynom3D::DerivateYDegree10,fgMagForceY->GetXmin(),fgMagForceY->GetXmax(),fgMagForceY->GetYmin(),fgMagForceY->GetYmax(),fgMagForceY->GetZmin(),fgMagForceY->GetZmax(),fgMagForceY->GetNpar() );
        fgMagForceYdY->SetParameters ( parY );
        fgMagForceYdZ = new TF3 ( "YdZ",&CpPolynom3D::DerivateZDegree10,fgMagForceY->GetXmin(),fgMagForceY->GetXmax(),fgMagForceY->GetYmin(),fgMagForceY->GetYmax(),fgMagForceY->GetZmin(),fgMagForceY->GetZmax(),fgMagForceY->GetNpar() );
        fgMagForceYdZ->SetParameters ( parY );

        fgMagForceZdX = new TF3 ( "ZdX",&CpPolynom3D::DerivateXDegree10,fgMagForceZ->GetXmin(),fgMagForceZ->GetXmax(),fgMagForceZ->GetYmin(),fgMagForceZ->GetYmax(),fgMagForceZ->GetZmin(),fgMagForceZ->GetZmax(),fgMagForceZ->GetNpar() );
        fgMagForceZdX->SetParameters ( parZ );
        fgMagForceZdY = new TF3 ( "ZdY",&CpPolynom3D::DerivateYDegree10,fgMagForceZ->GetXmin(),fgMagForceZ->GetXmax(),fgMagForceZ->GetYmin(),fgMagForceZ->GetYmax(),fgMagForceZ->GetZmin(),fgMagForceZ->GetZmax(),fgMagForceZ->GetNpar() );
        fgMagForceZdY->SetParameters ( parZ );
        fgMagForceZdZ = new TF3 ( "ZdZ",&CpPolynom3D::DerivateZDegree10,fgMagForceZ->GetXmin(),fgMagForceZ->GetXmax(),fgMagForceZ->GetYmin(),fgMagForceZ->GetYmax(),fgMagForceZ->GetZmin(),fgMagForceZ->GetZmax(),fgMagForceZ->GetNpar() );
        fgMagForceZdZ->SetParameters ( parZ );


        // second derivates
        fgMagForceXdX2 = new TF3 ( "XdX2",&CpPolynom3D::Derivate2XDegree10,fgMagForceX->GetXmin(),fgMagForceX->GetXmax(),fgMagForceX->GetYmin(),fgMagForceX->GetYmax(),fgMagForceX->GetZmin(),fgMagForceX->GetZmax(),fgMagForceX->GetNpar() );
        fgMagForceXdX2->SetParameters ( parX );
        fgMagForceXdY2 = new TF3 ( "XdY2",&CpPolynom3D::Derivate2YDegree10,fgMagForceX->GetXmin(),fgMagForceX->GetXmax(),fgMagForceX->GetYmin(),fgMagForceX->GetYmax(),fgMagForceX->GetZmin(),fgMagForceX->GetZmax(),fgMagForceX->GetNpar() );
        fgMagForceXdY2->SetParameters ( parX );
        fgMagForceXdZ2 = new TF3 ( "XdZ2",&CpPolynom3D::Derivate2ZDegree10,fgMagForceX->GetXmin(),fgMagForceX->GetXmax(),fgMagForceX->GetYmin(),fgMagForceX->GetYmax(),fgMagForceX->GetZmin(),fgMagForceX->GetZmax(),fgMagForceX->GetNpar() );
        fgMagForceXdZ2->SetParameters ( parX );

        fgMagForceYdX2 = new TF3 ( "YdX2",&CpPolynom3D::Derivate2XDegree10,fgMagForceY->GetXmin(),fgMagForceY->GetXmax(),fgMagForceY->GetYmin(),fgMagForceY->GetYmax(),fgMagForceY->GetZmin(),fgMagForceY->GetZmax(),fgMagForceY->GetNpar() );
        fgMagForceYdX2->SetParameters ( parY );
        fgMagForceYdY2 = new TF3 ( "YdY2",&CpPolynom3D::Derivate2YDegree10,fgMagForceY->GetXmin(),fgMagForceY->GetXmax(),fgMagForceY->GetYmin(),fgMagForceY->GetYmax(),fgMagForceY->GetZmin(),fgMagForceY->GetZmax(),fgMagForceY->GetNpar() );
        fgMagForceYdY2->SetParameters ( parY );
        fgMagForceYdZ2 = new TF3 ( "YdZ2",&CpPolynom3D::Derivate2ZDegree10,fgMagForceY->GetXmin(),fgMagForceY->GetXmax(),fgMagForceY->GetYmin(),fgMagForceY->GetYmax(),fgMagForceY->GetZmin(),fgMagForceY->GetZmax(),fgMagForceY->GetNpar() );
        fgMagForceYdZ2->SetParameters ( parY );

        fgMagForceZdX2 = new TF3 ( "ZdX2",&CpPolynom3D::Derivate2XDegree10,fgMagForceZ->GetXmin(),fgMagForceZ->GetXmax(),fgMagForceZ->GetYmin(),fgMagForceZ->GetYmax(),fgMagForceZ->GetZmin(),fgMagForceZ->GetZmax(),fgMagForceZ->GetNpar() );
        fgMagForceZdX2->SetParameters ( parZ );
        fgMagForceZdY2 = new TF3 ( "ZdY2",&CpPolynom3D::Derivate2YDegree10,fgMagForceZ->GetXmin(),fgMagForceZ->GetXmax(),fgMagForceZ->GetYmin(),fgMagForceZ->GetYmax(),fgMagForceZ->GetZmin(),fgMagForceZ->GetZmax(),fgMagForceZ->GetNpar() );
        fgMagForceZdY2->SetParameters ( parZ );
        fgMagForceZdZ2 = new TF3 ( "ZdZ2",&CpPolynom3D::Derivate2ZDegree10,fgMagForceZ->GetXmin(),fgMagForceZ->GetXmax(),fgMagForceZ->GetYmin(),fgMagForceZ->GetYmax(),fgMagForceZ->GetZmin(),fgMagForceZ->GetZmax(),fgMagForceZ->GetNpar() );
        fgMagForceZdZ2->SetParameters ( parZ );


    }
}


int CpFunctions::GslFunc ( double t, const double Ip[], double f[], void* /*params*/ ) {
//    printf("Hi\n");

//     printf("GslFunc:: x=%e y=%e z=%e\n",Ip[0],Ip[1],Ip[2]);
    f[0] = CpFunctions::dIp0 ( t,Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    f[1] = CpFunctions::dIp1 ( t,Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    f[2] = CpFunctions::dIp2 ( t,Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    f[3] = CpFunctions::dIp3 ( t,Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    f[4] = CpFunctions::dIp4 ( t,Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    f[5] = CpFunctions::dIp5 ( t,Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );

    return GSL_SUCCESS;
}

int CpFunctions::GslJac ( double t, const double Ip[], double* dfdy, double dfdt[], void* /*params*/ ) {
//    double *mu = (double *)params;
    printf("jac mu\n");



    gsl_matrix_view dfdy_mat = gsl_matrix_view_array ( dfdy, 6, 6 );
    gsl_matrix * m = &dfdy_mat.matrix;
    // Jakobyjan robime parcialna derivacia fo/parcialna derivacia Y1; parcialna derivacia f0/parcialna derivacia Y2...
    // prve 6 rovnci platia pre f0, dalsie 6 rovnice pre f1, ...

    gsl_matrix_set ( m, 0, 0, CpFunctions::dF0D0 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 0, 1, CpFunctions::dF0D1 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 0, 2, CpFunctions::dF0D2 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 0, 3, CpFunctions::dF0D3 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 0, 4, CpFunctions::dF0D4 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 0, 5, CpFunctions::dF0D5 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 1, 0, CpFunctions::dF1D0 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 1, 1, CpFunctions::dF1D1 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 1, 2, CpFunctions::dF1D2 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 1, 3, CpFunctions::dF1D3 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 1, 4, CpFunctions::dF1D4 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 1, 5, CpFunctions::dF1D5 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 2, 0, CpFunctions::dF2D0 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 2, 1, CpFunctions::dF2D1 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 2, 2, CpFunctions::dF2D2 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 2, 3, CpFunctions::dF2D3 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 2, 4, CpFunctions::dF2D4 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 2, 5, CpFunctions::dF2D5 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 3, 0, CpFunctions::dF3D0 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 3, 1, CpFunctions::dF3D1 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 3, 2, CpFunctions::dF3D2 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 3, 3, CpFunctions::dF3D3 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 3, 4, CpFunctions::dF3D4 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 3, 5, CpFunctions::dF3D5 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 4, 0, CpFunctions::dF4D0 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 4, 1, CpFunctions::dF4D1 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 4, 2, CpFunctions::dF4D2 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 4, 3, CpFunctions::dF4D3 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 4, 4, CpFunctions::dF4D4 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 4, 5, CpFunctions::dF4D5 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 5, 0, CpFunctions::dF5D0 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 5, 1, CpFunctions::dF5D1 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 5, 2, CpFunctions::dF5D2 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 5, 3, CpFunctions::dF5D3 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 5, 4, CpFunctions::dF5D4 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );
    gsl_matrix_set ( m, 5, 5, CpFunctions::dF5D5 ( Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] ) );

    dfdt[0] = CpFunctions::df0dt ( t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    dfdt[1] = CpFunctions::df1dt ( t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    dfdt[2] = CpFunctions::df2dt ( t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    dfdt[3] = CpFunctions::df3dt ( t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    dfdt[4] = CpFunctions::df4dt ( t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    dfdt[5] = CpFunctions::df5dt ( t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5] );
    return GSL_SUCCESS;
}

Double_t CpFunctions::dIp0 ( Double_t /*t*/, Double_t /*x*/, Double_t /*y*/, Double_t /*z*/, Double_t vx, Double_t /*vy*/, Double_t /*vz*/ ) {
    switch ( fgType ) {
    case kReal:
        return vx;
    case kRealDimLess:
      return vx;
    case kTest:
        return vx;
    default:
        return 0;
    }
}

Double_t CpFunctions::dIp1 ( Double_t /*t*/,Double_t /*x*/, Double_t /*y*/, Double_t /*z*/, Double_t /*vx*/, Double_t vy, Double_t /*vz*/ ) {
    switch ( fgType ) {
    case kReal:
        return vy;
    case kRealDimLess:
      return vy;
    case kTest:
        return vy;
    default:
        return 0;
    }
}

Double_t CpFunctions::dIp2 ( Double_t /*t*/,Double_t /*x*/, Double_t /*y*/, Double_t /*z*/, Double_t /*vx*/, Double_t /*vy*/, Double_t vz ) {
    switch ( fgType ) {
    case kReal:
        return vz;
    case kRealDimLess:
      return vz;
    case kTest:
        return vz;
    default:
        return 0;
    }
}

Double_t CpFunctions::dIp3 ( Double_t /*t*/,Double_t x, Double_t y, Double_t z, Double_t vx, Double_t vy, Double_t vz ) {
    Double_t chi = 0;
    Double_t res = 0;
//    printf("magx=%e magy=%e magz=%e\n",x,y,z);
    switch ( fgType ) {
    case kReal:
        chi = Chi(x,y,z);
        res = - 6*TMath::Pi()*kBloodViscosity*kParticleR*(vx-kBloodVelX);
        if (chi<1e-10) {
//             printf("X CHI[0] %e [%e %e %e] %e\n",chi,x,y,z,res);
            return res;
        }
        else {
            res+= - Chi(x,y,z)/kMuZero*4*TMath::Pi()*TMath::Power(kParticleR,3)*MyNabla1(x,y,z,vx,vy,vz)*kBulharConstant;
//             printf("X CHI %e [%e %e %e] %e %e\n",chi,x,y,z,res,kBulharConstant);
            return res;
        }

//         return -6*TMath::Pi() *kBloodViscosity*kParticleR/ParticleMass() * ( vx-kBloodVelX ) - kMuZero*kChi/ ( 4*TMath::Pi() )*MyNabla1(x,y,z,vx,vy,vz);
// printf("magx=%e magy=%e magz=%e res=%e\n",x,y,z,-6*TMath::Pi() * ( vx-kBloodVelX ) - ( 4*TMath::Pi() )*MyNabla1(x,y,z,vx,vy,vz));
//         return -6*TMath::Pi() * ( vx-kBloodVelX ) - ( 4*TMath::Pi() )*MyNabla1(x,y,z,vx,vy,vz);
// return -kMuZero*kChi/ ( 4*TMath::Pi() ) *MyNabla1(x,y,z,vx,vy,vz);
        return 0;
    case kTest:
        return 0;
    default:
        return 0;
    }
}
Double_t CpFunctions::dIp4 ( Double_t t,Double_t x, Double_t y, Double_t z, Double_t vx, Double_t vy, Double_t vz ) {
    Double_t chi = 0;
    Double_t res = 0;
    switch ( fgType ) {
    case kReal:

        chi = Chi(x,y,z);
        res = - 6*TMath::Pi()*kBloodViscosity*kParticleR*(vy-kBloodVelY);
        if (chi<1e-20) {
//             printf("Y CHI[0] %e [%e %e %e] %e\n",chi,x,y,z,res);
            return res;
        }
        else {
            res+= - Chi(x,y,z)/kMuZero*4*TMath::Pi()*TMath::Power(kParticleR,3)*MyNabla2(x,y,z,vx,vy,vz)*kBulharConstant;
//             printf("Y CHI %e [%e %e %e] %e %e\n",chi,x,y,z,res,kBulharConstant);
            return res;
        }
//       if (chi<1e-10) return - 6*TMath::Pi()*kBloodViscosity*kParticleR*(vy-kBloodVelY);
//       else return - 6*TMath::Pi()*kBloodViscosity*kParticleR*(vy-kBloodVelY) - Chi(x,y,z)/kMuZero*4*TMath::Pi()*TMath::Power(kParticleR,3)*MyNabla2(x,y,z,vx,vy,vz)*kBulharConstant;
        //        return -6*TMath::Pi() *kBloodViscosity*kParticleR/ParticleMass() * ( vy-kBloodVelY )-kMuZero*kChi/ ( 4*TMath::Pi() )*MyNabla2(x,y,z,vx,vy,vz);
//        return -6*TMath::Pi()  * ( vy-kBloodVelY )-( 4*TMath::Pi() )*MyNabla2(x,y,z,vx,vy,vz);
// return -kMuZero*kChi/ ( 4*TMath::Pi() ) *MyNabla2(x,y,z,vx,vy,vz);
        return 0;
    case kTest:
        return -TMath::Power(TMath::Pi()/100,2)*TMath::Sin(TMath::Pi()*t/100);
    default:
        return 0;
    }
}
Double_t CpFunctions::dIp5 ( Double_t t,Double_t x, Double_t y, Double_t z, Double_t vx, Double_t vy, Double_t vz ) {

    Double_t chi = 0;
    Double_t res = 0;
    switch ( fgType ) {
    case kReal:

        chi = Chi(x,y,z);
        res = - 6*TMath::Pi()*kBloodViscosity*kParticleR*(vz-kBloodVelZ);
        if (chi<1e-10) {
//             printf("Z CHI[0] %e [%e %e %e] %e\n",chi,x,y,z,res);
            return res;
        }
        else {
            res+= - Chi(x,y,z)/kMuZero*4*TMath::Pi()*TMath::Power(kParticleR,3)*MyNabla2(x,y,z,vx,vy,vz)*kBulharConstant;
//             printf("X CHI %e [%e %e %e] %e %e\n",chi,x,y,z,res,kBulharConstant);
            return res;
        }
//       if (chi<1e-10) return - 6*TMath::Pi()*kBloodViscosity*kParticleR*(vz-kBloodVelZ);
//       else return - 6*TMath::Pi()*kBloodViscosity*kParticleR*(vz-kBloodVelZ) - Chi(x,y,z)/kMuZero*4*TMath::Pi()*TMath::Power(kParticleR,3)*MyNabla3(x,y,z,vx,vy,vz)*kBulharConstant;
//        return -6*TMath::Pi() *kBloodViscosity*kParticleR/ParticleMass() * ( vz-kBloodVelZ )-kMuZero*kChi/ ( 4*TMath::Pi() )*MyNabla3(x,y,z,vx,vy,vz);
// return -kMuZero*kChi/ ( 4*TMath::Pi() ) *MyNabla3(x,y,z,vx,vy,vz);
//       return -6*TMath::Pi() * ( vz-kBloodVelZ )- ( 4*TMath::Pi() )*MyNabla3(x,y,z,vx,vy,vz);
        return 0;
    case kTest:
        return -TMath::Power(TMath::Pi()/100,2)*TMath::Cos(TMath::Pi()*t/100);
    default:
        return 0;
    }
}

Double_t CpFunctions::MyNabla1 ( Double_t x, Double_t y, Double_t z, Double_t /*vx*/, Double_t /*vy*/, Double_t /*vz*/ )
{

    if (!fgIsInMaget) return 0.0;

    switch ( fgType ) {
      case kRealDimLess:
        gGeoManager->SetCurrentPoint(x*kMultipier*kParticleR,y*kMultipier*kParticleR,z*kMultipier*kParticleR);
      default:
        gGeoManager->SetCurrentPoint(x,y,z);
    }
    const Double_t *glob_pt = gGeoManager->GetCurrentPoint();
    Double_t local_pt[3];
    gGeoManager->MasterToLocal(glob_pt, local_pt);
    // conver XYZ to Magnet coordinate
    Double_t magX=local_pt[0]*100;
    Double_t magY=local_pt[1]*100;
    Double_t magZ=local_pt[2]*100;


        

//     TGeoNode*nextNode = gGeoManager->Step(kFALSE);
//     if (nextNode) printf("%s [%e %e %e]  [%e %e %e] %e\n",nextNode->GetName(),x,y,z,magX,magY,magZ,fgMagForceX->Eval(magX,magY,magZ)*fgMagForceXdX->Eval(magX,magY,magZ)+fgMagForceY->Eval(magX,magY,magZ)*fgMagForceXdY->Eval(magX,magY,magZ)+fgMagForceZ->Eval(magX,magY,magZ)*fgMagForceXdZ->Eval(magX,magY,magZ));

//    Double_t magX=x;
//    Double_t magY=y;
//    Double_t magZ=z;
//
//    if (gGeoManager) printf("We have geoManager\n");

//    printf("magx=%e magy=%e magz=%e\n",magX,magY,magZ);

    return fgMagForceX->Eval(magX,magY,magZ)*fgMagForceXdX->Eval(magX,magY,magZ)+fgMagForceY->Eval(magX,magY,magZ)*fgMagForceXdY->Eval(magX,magY,magZ)+fgMagForceZ->Eval(magX,magY,magZ)*fgMagForceXdZ->Eval(magX,magY,magZ);
}
Double_t CpFunctions::MyNabla2 ( Double_t x, Double_t y, Double_t z, Double_t /*vx*/, Double_t /*vy*/, Double_t /*vz*/ )
{
    if (!fgIsInMaget) return 0.0;

    switch ( fgType ) {
      case kRealDimLess:
        gGeoManager->SetCurrentPoint(x*kMultipier*kParticleR,y*kMultipier*kParticleR,z*kMultipier*kParticleR);
      default:
        gGeoManager->SetCurrentPoint(x,y,z);
    }
    const Double_t *glob_pt = gGeoManager->GetCurrentPoint();
    Double_t local_pt[3];
    gGeoManager->MasterToLocal(glob_pt, local_pt);
    // conver XYZ to Magnet coordinate
    Double_t magX=local_pt[0]*100;
    Double_t magY=local_pt[1]*100;
    Double_t magZ=local_pt[2]*100;

// Double_t magX=x;
// Double_t magY=y;
// Double_t magZ=z;

    return fgMagForceY->Eval(magX,magY,magZ)*fgMagForceYdX->Eval(magX,magY,magZ)+fgMagForceY->Eval(magX,magY,magZ)*fgMagForceYdY->Eval(magX,magY,magZ)+fgMagForceZ->Eval(magX,magY,magZ)*fgMagForceYdZ->Eval(magX,magY,magZ);
}
Double_t CpFunctions::MyNabla3 ( Double_t x, Double_t y, Double_t z, Double_t /*vx*/, Double_t /*vy*/, Double_t /*vz*/ )
{

    if (!fgIsInMaget) return 0.0;
    
    switch ( fgType ) {
      case kRealDimLess:
        gGeoManager->SetCurrentPoint(x*kMultipier*kParticleR,y*kMultipier*kParticleR,z*kMultipier*kParticleR);
      default:
        gGeoManager->SetCurrentPoint(x,y,z);
    }
    
    const Double_t *glob_pt = gGeoManager->GetCurrentPoint();
    Double_t local_pt[3];
    gGeoManager->MasterToLocal(glob_pt, local_pt);
    // conver XYZ to Magnet coordinate
    Double_t magX=local_pt[0]*100;
    Double_t magY=local_pt[1]*100;
    Double_t magZ=local_pt[2]*100;


//   Double_t magX=x;
//   Double_t magY=y;
//   Double_t magZ=z;
    return fgMagForceX->Eval(magX,magY,magZ)*fgMagForceZdX->Eval(magX,magY,magZ)+fgMagForceY->Eval(magX,magY,magZ)*fgMagForceZdY->Eval(magX,magY,magZ)+fgMagForceZ->Eval(magX,magY,magZ)*fgMagForceZdZ->Eval(magX,magY,magZ);
}

void CpFunctions::PrintMagneticInfo ( Double_t x, Double_t y, Double_t z )
{


    if ( fgMagForceX ) {
        printf ( "X is %f \n",fgMagForceX->Eval ( x,y,z ) );
        printf ( "dX/dx is %f \n",fgMagForceXdX->Eval ( x,y,z ) );
        printf ( "dX/dy is %f \n",fgMagForceXdY->Eval ( x,y,z ) );
        printf ( "dX/dz is %f \n",fgMagForceXdZ->Eval ( x,y,z ) );
        printf ( "dX/dx2 is %f \n",fgMagForceXdX->Eval ( x,y,z ) );
        printf ( "dX/dy2 is %f \n",fgMagForceXdY2->Eval ( x,y,z ) );
        printf ( "dX/dz2 is %f \n",fgMagForceXdZ2->Eval ( x,y,z ) );
    }
    if ( fgMagForceY ) {
        printf ( "Y is %f \n",fgMagForceY->Eval ( x,y,z ) );
        printf ( "dY/dx is %f \n",fgMagForceYdX->Eval ( x,y,z ) );
        printf ( "dY/dy is %f \n",fgMagForceYdY->Eval ( x,y,z ) );
        printf ( "dY/dz is %f \n",fgMagForceYdX->Eval ( x,y,z ) );
        printf ( "dY/dx2 is %f \n",fgMagForceYdX2->Eval ( x,y,z ) );
        printf ( "dY/dy2 is %f \n",fgMagForceYdY2->Eval ( x,y,z ) );
        printf ( "dY/dz2 is %f \n",fgMagForceYdX2->Eval ( x,y,z ) );
    }
    if ( fgMagForceZ ) {
        printf ( "Z is %f \n",fgMagForceZ->Eval ( x,y,z ) );
        printf ( "dZ/dx is %f \n",fgMagForceZdX->Eval ( x,y,z ) );
        printf ( "dZ/dy is %f \n",fgMagForceZdY->Eval ( x,y,z ) );
        printf ( "dZ/dz is %f \n",fgMagForceZdZ->Eval ( x,y,z ) );
        printf ( "dZ/dx2 is %f \n",fgMagForceZdX2->Eval ( x,y,z ) );
        printf ( "dZ/dy2 is %f \n",fgMagForceZdY2->Eval ( x,y,z ) );
        printf ( "dZ/dz2 is %f \n",fgMagForceZdZ2->Eval ( x,y,z ) );

    }
}
Double_t CpFunctions::Chi(Double_t x, Double_t y, Double_t z)
{

    // points has to be in Magnet coordinates
    // TODO Convert it

    if (!fgIsInMaget) return 0.0;

    gGeoManager->SetCurrentPoint(x,y,z);
    const Double_t *glob_pt = gGeoManager->GetCurrentPoint();
    Double_t local_pt[3];
    gGeoManager->MasterToLocal(glob_pt, local_pt);
// conver XYZ to Magnet coordinate
    Double_t magX=local_pt[0]*100;
    Double_t magY=local_pt[1]*100;
    Double_t magZ=local_pt[2]*100;
    Double_t xp[] = {magX,magY,magZ};
    if (!fgMagForceX->IsInside(xp)) return 0;

    return kIs/TMath::Sqrt(TMath::Power(fgMagForceX->Eval(magX,magY,magZ),2)+TMath::Power(fgMagForceY->Eval(magX,magY,magZ),2)+TMath::Power(fgMagForceZ->Eval(magX,magY,magZ),2));
}

Double_t CpFunctions::BloodVelocityAverage()
{
  return 1e-3;
}
